/******************************************************************************
 *  _____     _____
 *  \  __\    \  __\
 *   \ \_\\    \ \_\\
 *    \  __\SS  \  __\roject
 *     \_\       \_\
 *
 * Mateusz Maciąg, Rafał Urbański
 * Komputerowe Systemy Sterowania
 *****************************************************************************/

#include "RmnkSolver.h"

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include <algorithm>
#include <numeric>

#include <iostream>
#include <iomanip>
#include <fstream>
#include <stdexcept>

#include <cmath>

#include "Utils.h"

using namespace std;

namespace RafMat
{
namespace SimEngine
{

// Do debugowania:
//static void printVect1(ostream& o, const std::string n, const RmnkSolver::Matrix& m)
//{
//    o << n << ":[ ";
//    RmnkSolver::Matrix::const_iterator1 it;
//    for(it =m.begin1(); it != m.end1(); ++it)
//        o << *it << ' ';
//    o << "]" << endl;
//}

//static void printVect2(ostream& o, const std::string n, const RmnkSolver::Matrix& m)
//{
//    o << n << ":[ ";
//    RmnkSolver::Matrix::const_iterator2 it;
//    for(it =m.begin2(); it != m.end2(); ++it)
//        o << *it << ' ';
//    o << "]" << endl;
//}

//static void printMat(ostream& o, const std::string n, const RmnkSolver::Matrix& m)
//{
//    o << n << ":[ ";
//    for(unsigned i = 0; i < m.size1(); i++)
//    {
//        for(unsigned j = 0; j < m.size2(); j++)
//        {
//            cout << m(i, j) << ' ';
//        }
//        cout << '\n';
//    }
//    o << "]" << endl;
//}

// Stałe
const double RmnkSolver::INITIAL_COVARIANCE_FACTOR = 1000;
const double RmnkSolver::OMEGA_FACTOR = 1000;

RmnkSolver::RmnkSolver(unsigned rank, double lambda, double beta, double T)
    : m_rankSum(rank),
      m_lambda(Utils::limitValue(lambda, 0.0, 1.0)),
      m_beta(beta),
      m_traceLimit(T)
{
    this->reset();
}

void RmnkSolver::reset()
{
    /*
    vector<double> tmp;
    tmp.push_back(0.4);
    tmp.push_back(0.6);
    pb = Polynomial(tmp);
    tmp[0] = 1;
    tmp[1] = -0.8;
    pa = Polynomial(tmp);
    */

    // 3. Wektor wzmocnień
    m_gainVect = Matrix(m_rankSum, 1);

    // 4. Macierz kowariancji + macierze pomocnicze.
    // Macierz kowariancji jest macierzą kwadratową
    // o wymiarze dA + dB. Zgodnie z warunkami początkowymi algorytmu
    // ważonego RMNK początkowo jest to macierz jednostkowa razy pewien
    // duży współczynnik.
    m_covariance = IdentityMatrix(m_rankSum) * INITIAL_COVARIANCE_FACTOR;
    m_covarianceN = Matrix(m_rankSum, m_rankSum);
    m_covarianceD = Matrix(m_rankSum, m_rankSum);

    m_noiseEstimator = 0.5;

    //printMat(cout, "covSta", m_covariance);
}

// Ślad macierzy
//double RmnkSolver::trace(const Matrix& mat)
//{
//    BOOST_ASSERT(mat.size1() == mat.size2());

//    double accum = 0.0;
//    for(unsigned i = 0; i<mat.size1(); i++)
//    {
//        accum += mat(i,i);
//    }
//    return accum;
//}

bool RmnkSolver::invertMatrix(const Matrix& input, Matrix& inv)
{
    typedef boost::numeric::ublas::permutation_matrix<std::size_t> pmatrix;
    using boost::numeric::ublas::lu_factorize;
    using boost::numeric::ublas::lu_substitute;

    // Utwórz kopię roboczą wejściowej macierzy
    Matrix A(input);

    // Utwórz macierz permutacji potrzebną do rozkładu LU.
    pmatrix pm(A.size1());

    // Rozkład LU
    int res = lu_factorize(A, pm);
    if (res != 0)
        return false;

    // Utwórz macierz jednostkową i przypisz go macierzy odwróconej.
    inv.assign(IdentityMatrix (A.size1()));

    // Podstawienie wg algorytmu LU w celu uzyskania macierzy odwróconej.
    lu_substitute(A, pm, inv);

    return true;
}

// Realizacja metody RMNK z kierunkowym zapominaniem
using namespace boost::numeric;
RmnkSolver::Matrix RmnkSolver::doForgettingMethod(const Matrix& varphi,
                                                  Matrix &prevCovariance)
{
    static const double MINIMUM_DENOMINATOR_VAL = 1e-5;

    double ki = 0;
    double denominator = MINIMUM_DENOMINATOR_VAL;

    // Macierze buforowe pomocnicze:
    /// TODO: Umieścić macierze bufMat* do pól prywatnych, bo za każdym razem tworzone są na nowo.
    Matrix bufMat1(m_rankSum, m_rankSum);
    Matrix bufMat2(m_rankSum, m_rankSum);

    Matrix bufVect;

    // Wyznaczenie macierzy P^-1(i).
    this->invertMatrix(prevCovariance, bufMat1);

    bufMat2 = prod(varphi, ublas::trans(varphi));

    // Równ. (32.19) [D.B.]
    bufMat1 = bufMat1 + bufMat2;

    //Mianownika z równ. (32.21) [D.B.]
    bufVect = ublas::prod(trans(varphi), prevCovariance);

    //phiT(i-1) * P(i-1) * phi(i-1)
    denominator = ublas::prod(bufVect, varphi)(0,0);

    // Ograniczamy wartość mianownika, bo wiadomo co odwrotność robi:
    if(abs(denominator) < MINIMUM_DENOMINATOR_VAL)
        denominator = MINIMUM_DENOMINATOR_VAL;

    // Wartość k(i) równ. (32.21) [D.B.]
    ki = (1.0 - m_lambda) * (1.0 + 1.0/denominator);

    // Macierzy P^-1(i) równ. (32.20) [D.B.]
    bufMat2 = ublas::prod(varphi, trans(varphi));
    bufMat1 = bufMat1 - ki * bufMat2;

    //Wyznaczenie wartości macierzy P(i) poprzez odwrócenie macierzy P^-1(i)
    invertMatrix(bufMat1, bufMat1);

    return bufMat1;
}

RmnkSolver::Matrix RmnkSolver::identify(const Matrix& theta, const Matrix& phi, double outSample)
{
    if(theta.size1() < m_rankSum)
        throw std::runtime_error("RmnkSolver: Invalid dimmension of theta.");

    // prod - mnożenie macierzy, trans - transpozycja
    using boost::numeric::ublas::prod;
    using boost::numeric::ublas::trans;

    double eps;

//    printVect1(std::cout, "theta(i) = ", theta);
//    printVect1(std::cout, "phi(i) = ", phi);

    // Wykonujemy metodę kierunkowego zapominania:
    m_covarianceN = doForgettingMethod(phi, m_covariance);
    m_gainVect = prod(m_covarianceN, phi);

    // Błąd predykcji:
    eps = outSample - ublas::prod(ublas::trans(theta), phi)(0,0);

    // Zwróć nową ocenę wektora paramerów
    return theta + m_gainVect * eps;
}

//void RmnkSolver::identify(double inSample, double outSample)
//{
//    //static ofstream ofstr("output.txt");

//    // prod - mnożenie macierzy, trans - transpozycja
//    using boost::numeric::ublas::prod;
//    using boost::numeric::ublas::trans;

//    static double eps = outSample;

//    // Zapamiętujemy próbkę wejściową
//    m_inmem.push_front(inSample);
//    m_inmem.pop_back();

//    // Przygotowujemy wektor varphi z identyfikowanymi parametrami:
//    Matrix::iterator1 bnd_it = m_varphi.begin1() + m_dB + 1; // granica między u i -y.
//    copy(m_inmem.begin() + m_delay, m_inmem.end(), m_varphi.begin1());
//    copy(m_outmem.begin(), m_outmem.end(), bnd_it);
//    transform(bnd_it, m_varphi.end1(), bnd_it, negate<double>());

//    // Wykonujemy metodę kierunkowego zapominania:
//    m_covarianceN = doForgettingMethod(m_covariance, m_varphi);

//    m_gainVect = prod(m_covarianceN, m_varphi);
//    eps = outSample - ublas::prod(ublas::trans(m_theta), m_varphi)(0,0);
//    m_theta += m_gainVect * eps;

//    copy(m_theta.begin1(), m_theta.begin1() + m_dB + 1, m_estimatedB.begin());
//    copy(m_theta.begin1() + m_dB + 1, m_theta.end1(), m_estimatedA.begin()+1);

//    // Dibag:
//    //printVect1(cout, "theta", m_theta);

//    // Zapamiętujemy próbkę wyjściową
//    m_outmem.push_front(outSample);
//    m_outmem.pop_back();
//}

//
// ŚMIETNIK
//

// Werjsa I - nie działa
/*
void RmnkSolver::identify(double inSample, double outSample)
{
    //static ofstream ofstr("output.txt");

    using boost::numeric::ublas::prod;
    using boost::numeric::ublas::trans;

    // Wstawienie nowych próbek do pamięci, usunięcie starych
    m_inmem.push_front(inSample);
    m_inmem.pop_back();

    // Wektor varphi zbudowany jest z wejść i zanegowanych wyjść
    // wg równania na y(i) = ... w modelu ARX.

    // KROKI ALGORYTMU:
    // 1. Zbudowanie wektora varphi = [ u(i-k) u(i-k-1) ... u(i-k-dB) -y(i-1) -y(i-2) ... -y(i-dA) ]
    Matrix::iterator1 bnd_it = m_varphi.begin1() + m_dB + 1; // granica między u i -y.
    copy(m_inmem.begin() + m_delay, m_inmem.end(), m_varphi.begin1());
    copy(m_outmem.begin(), m_outmem.end(), bnd_it);
    transform(bnd_it, m_varphi.end1(), bnd_it, negate<double>());

    cout << setprecision(2);

    // 2. Obliczenie transpozycji (przydadzą się).
    Matrix varphiT = trans(m_varphi);
    Matrix thetaT = trans(m_theta);

    // DEBUG:
    //printVect1(cout, "theta", m_theta);

    // 3. Obliczenie błędu identyfikacji. Wynikiem prod jest macierz 1x1, trzeba
    // ręcznie dobrać się do jego jedynego elementu (0,0).
    double eps = outSample - prod(thetaT, m_varphi)(0,0);
    double epsSquared = eps * eps;

    // 4. Obliczenie nowego estymatora wariancji szumu zakłócenia.
    m_noiseEstimator = sqrt(m_noiseEstimator * m_noiseEstimator * m_beta + (1-m_beta) * epsSquared);

    double omega = m_noiseEstimator * OMEGA_FACTOR;

    // 5. Obliczenie wspólnego dość dzikiego wyrażenia macierzowego (wynikiem jest skalar).
    m_covarianceD = prod(varphiT, m_covariance);
    m_covarianceD = prod(m_covarianceD, m_varphi);
    double den = 1 + m_covarianceD(0,0);

    // 6. Obliczenie nowego współczynnika zapominania:
    m_lambda = Utils::limitValue(1 - epsSquared / (den * omega), 0.0, 1.0);

    // 7. Obliczenie wektora wzmocnień:
    m_gainVect = prod(m_covariance, m_varphi) / den;

    printVect1(cout, "g", m_gainVect);

    // 8. Obliczenie macierzy kowariancji:
    m_covarianceN = prod(m_gainVect, varphiT);
    m_covarianceN = prod(m_covarianceN, m_covariance);
    m_covarianceD = m_covarianceN / m_lambda;

    if(trace(m_covarianceD) <= m_traceLimit)
        m_covariance = m_covarianceD;
    else
        m_covariance = m_covarianceN;

    printMat(cout, "cov", m_covariance);

    // 9. Obliczenie nowych parametrów modelu ARX:
    m_theta += m_gainVect * eps;

    // Kopiujemy wyniki obliczeń do wielomianów A i B:
    copy(m_theta.begin1(), m_theta.begin1() + m_dB, m_estimatedB.begin());
    copy(m_theta.begin1() + m_dB + 1, m_theta.end1(), m_estimatedA.begin()+1);

    // Dopiero teraz uaktualniamy pamięć wyjść.
    m_outmem.push_front(outSample);
    m_outmem.pop_back();
}*/

// Wersja II - działa, ale nie do końca
//void RmnkSolver::identify(double inSample, double outSample)
//{
//    //static ofstream ofstr("output.txt");

//    using boost::numeric::ublas::prod;
//    using boost::numeric::ublas::trans;

//    // Wstawienie nowych próbek do pamięci, usunięcie starych
//    m_inmem.push_front(inSample);
//    m_inmem.pop_back();

//    // Wektor varphi zbudowany jest z wejść i zanegowanych wyjść
//    // wg równania na y(i) = ... w modelu ARX.

//    // KROKI ALGORYTMU:
//    // 1. Zbudowanie wektora varphi = [ u(i-k) u(i-k-1) ... u(i-k-dB) -y(i-1) -y(i-2) ... -y(i-dA) ]
//    Matrix::iterator1 bnd_it = m_varphi.begin1() + m_dB + 1; // granica między u i -y.
//    copy(m_inmem.begin() + m_delay, m_inmem.end(), m_varphi.begin1());
//    copy(m_outmem.begin(), m_outmem.end(), bnd_it);
//    transform(bnd_it, m_varphi.end1(), bnd_it, negate<double>());

//    cout << setprecision(2);

//    // 2. Obliczenie transpozycji (przydadzą się).
//    Matrix varphiT = trans(m_varphi);
//    Matrix thetaT = trans(m_theta);

//    printVect1(cout, "theta", m_theta);

//    // 3. Obliczenie błędu identyfikacji. Wynikiem prod jest macierz 1x1, trzeba
//    // ręcznie dobrać się do jego jedynego elementu (0,0).
//    double eps = outSample - prod(thetaT, m_varphi)(0,0);

//    // 4. Liczymy tą dziwną macierz kowariancji wg wzoru rekurencyjnego.
//    // Osobno liczymy licznik (N) i mianownik (D) tego dzikiego ułamka.
//    m_covarianceN = prod(m_covariance, m_varphi);
//    m_covarianceN = prod(m_covarianceN, varphiT);
//    m_covarianceN = prod(m_covarianceN, m_covariance);

//    // lambdę dodajemy do wyniku mnożenia macierzy. Efektywnie jest zastosować
//    // spacjalną macierz skalarną, która nie przechowuje żadnych tablic.
//    typedef boost::numeric::ublas::scalar_matrix<double> ScalarMatrix;

//    m_covarianceD = prod(varphiT, m_covariance);
//    m_covarianceD = prod(m_covarianceD, m_varphi) + ScalarMatrix(1, 1)*m_lambda;

//    // Obliczenie nowej macierzy kowariancji:
//    // Mianownik (D) powinien być skalarem, (N) powinien być macierzą o takich samych
//    // wymiarach co m_covariance.
//    m_covariance = (m_covariance - m_covarianceN / m_covarianceD(0,0));

//    // 5. Obliczenie wektora wzmocnień:
//    m_gainVect = prod(m_covariance, m_varphi);

//    // 6. Obliczenie nowych parametrów modelu ARX:
//    m_theta += m_gainVect * eps;

//    // Kopiujemy wyniki obliczeń do wielomianów A i B:
//    copy(m_theta.begin1(), m_theta.begin1() + m_dB + 1, m_estimatedB.begin());
//    copy(m_theta.begin1() + m_dB + 1, m_theta.end1(), m_estimatedA.begin()+1);

//    // Dopiero teraz uaktualniamy pamięć wyjść.
//    m_outmem.push_front(outSample);
//    m_outmem.pop_back();
//}


}
}
